from xgolib import XGO
import threading, time, cv2, struct
from PIL import Image
import xgoscreen.LCD_2inch as LCD_2inch

class XGOController:
    def __init__(self, xgo_device, conn):
        self.xgo = XGO(xgo_device)
        self.camera_still = False
        self.display = LCD_2inch.LCD_2inch()
        self.display.Init()
        self.display.clear()
        self.splash = Image.new("RGB", (320, 240), "black")
        self.display.ShowImage(self.splash)
        self.cap = None
        self.conn = conn

    def go(self, x):
        if x[0] == "f":
            self.xgo.move("x", 5)
        if x[0] == "s":
            self.xgo.stop()
        if x[0] == "b":
            self.xgo.move("x", -5)
        if x[0] == "r":
            self.xgo.move("y",-5)
        if x[0] == "l":
            self.xgo.move("y", 5)
        if x[0] == "tr":
            self.xgo.turn(-15)
        if x[0] == "tl":
            self.xgo.turn(15)
    def tran(self, x):
        if x[0] in "xyz":
            self.xgo.translation(x[0], int(x[1]))
        if x[0] in "pyr":
            self.xgo.attitude(x[0], int(x[1]))
    def arm(self,x):
        self.xgo.arm(int(x[0]), int(x[1]))

    def setpace(self, model):
        self.xgo.pace(model)

    def setcamera(self):
        if self.cap != None and  self.camera_still:
            self.camera_still = False
            time.sleep(0.5)
            splash = Image.new("RGB", (320, 240), "black")
            self.display.ShowImage(splash)
        else:
            self.open_camera()
            self.camera_still = True
            time.sleep(0.5)
            t = threading.Thread(target=self.camera_mode)
            t.start()
        return self.cap

    def open_camera(self):
        if self.cap == None:
            self.cap = cv2.VideoCapture(0)
            self.cap.set(3, 320)
            self.cap.set(4, 240)

    def camera_mode(self):
        self.camera_still=True
        while self.camera_still:
            success,frame = self.cap.read()
            b,g,r = cv2.split(frame)
            image = cv2.merge((r, g, b))
            image = cv2.flip(image,1)
            imgok = Image.fromarray(image)
            self.display.ShowImage(imgok)

            # socket发送数据
            img_encode = cv2.imencode('.jpg', frame)[1]
            str_encode = img_encode.tobytes()
            image_length = len(str_encode)
            packed_length = struct.pack('!I', image_length)
            self.conn.sendall(packed_length)
            self.conn.sendall(str_encode)

            if not self.camera_still:
                break